#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include "fft_math.h"

#define PI 3.1415926535

void FFT(float dataR[],float dataI[],float dataA[],int N,int M)
{

	int i,j,k,r;
	int p,L,B;
	unsigned int I,J,K,F0,F1,m,n;
	float Tr,Ti,temp;


	for(I=0;I< N;I++)
	{  
		J=0;
		for(k=0;k<(M/2+0.5);k++)
	     {
	         m=1;
			 n=(unsigned int)pow(2,M-1);
			 m <<= k;
	         n >>= k;
	         F0=I & n;
			 F1=I & m;
	         if(F0) J=J | m;
	         if(F1) J=J | n;
	     }

	 	 if(I<J)
			{
				temp = dataR[I];
				dataR[I] = dataR[J];
				dataR[J] = temp;
				temp = dataI[I];
				dataI[I] = dataI[J];
				dataI[J] = temp;
			}                                
	} 

	for(L=1; L<=M;L++)
	{		

		B = 1;
		B = (int)pow(2,L-1);	
		for(j=0;j<=B-1;j++)		
		//j = 0,1,2,...,2^(L-1) - 1
		{
			k=1;
			k = (int)pow(2,M-L);
			p=1;
			p=j*k;

			for(i=0; i<=k-1;i++) 
				{
					r=1;
					r=j+2*B*i;
					Tr=dataR[r+B]*cos(2.0*PI*p/N) + dataI[r+B]*sin(2.0*PI*p/N);
					Ti=dataI[r+B]*cos(2.0*PI*p/N) - dataR[r+B]*sin(2.0*PI*p/N);
					dataR[r+B]=dataR[r]-Tr;
					dataI[r+B]=dataI[r]-Ti;
					dataR[r]=dataR[r]+Tr;
					dataI[r]=dataI[r]+Ti;
				}	
			}
	}
	for ( i=0;i<N;i++ )
	{ 		
	 	dataA[i]=sqrt(dataR[i]*dataR[i]+dataI[i]*dataI[i]);
	}
}

void FFTR(float dataR[],float dataI[],float dataA[],int N,int M)
{
	int i,k;
	int j,r,p,B;
	float Tr,Ti;
	float yR[N/2],yI[N/2],yA[N/2]; 
	float x1R[N/2],x2R[N/2],x1I[N/2],x2I[N/2],xR[N],xI[N];
	for(i=0;i<N/2;i++)
	{
		yR[i]=dataR[2*i];
		yI[i]=dataR[2*i+1];
				
	}
	FFT(yR,yI,yA,N/2,M-1);
	for(k=0;k<N/2;k++)
	{
		if(k==0)
		{
			x1R[k]=yR[k];
			x1I[k]=yI[k];
			x2R[k]=yI[k];
			x2I[k]=-yR[k];	
		}
		else
		{
			x1R[k]=(yR[k]+yR[N/2-k])/2;
			x1I[k]=(yI[k]-yI[N/2-k])/2;
			x2R[k]=(yI[k]+yI[N/2-k])/2;
			x2I[k]=(yR[N/2-k]-yR[k])/2;
		}
	}
	
	B = 1;
	B = (int)pow(2,M-1);	
	for(j=0;j<=N/2-1;j++)		
	//j = 0,1,2,...,2^(L-1) - 1
	{		
		p=1;
		p=j;
		r=1;
		r=j;
		int k=j;
		Tr=x2R[r]*cos(2.0*PI*p/N) + x2I[r]*sin(2.0*PI*p/N);
		Ti=x2I[r]*cos(2.0*PI*p/N) - x2R[r]*sin(2.0*PI*p/N);
		xR[r]=x1R[r]+Tr;
		xI[r]=x1I[r]+Ti;

		if(r==0)
		{
			xR[N/2]=x1R[0]-x2R[0];
			xI[N/2]=x1I[0]-x2I[0];		
		}
		else
		{
			xR[N-r]=xR[r];
			xI[N-r]=-xI[r];	
		}
	}
	
	for(i=0;i<N;i++)
	{
		dataR[i]=xR[i];
		dataI[i]=xI[i];
	}
	for ( i=0;i<N;i++ )
	{ 		
		//dataA[i]=sqrt(xR[i]*xR[i]+xI[i]*xI[i]);
	 	dataA[i]=sqrt(dataR[i]*dataR[i]+dataI[i]*dataI[i]);
	}	
} 


void IFFT(float dataR[],float dataI[],float dataA[],int N,int M) 
{
	int i,j,k,r;
	int p,L,B;
	unsigned int I,J,K,F0,F1,m,n;
	float Tr,Ti,temp;

	for(I=0;I< N;I++)
	{  
		J=0;
		for(k=0;k<(M/2+0.5);k++)
	     {
	         
	         m=1;
			 n=(unsigned int)pow(2,M-1);
			 m <<= k;
	         n >>= k;
	         F0=I & n;
			 F1=I & m;
	         if(F0) J=J | m;
	         if(F1) J=J | n;
	     }

	 	 if(I<J)
			{
			temp = dataR[I];
			dataR[I] = dataR[J];
			dataR[J] = temp;
			temp = dataI[I];
			dataI[I] = dataI[J];
			dataI[J] = temp;
			}                                
	} 

	for(L=1; L<=M;L++)
	{		

		B = 1;
		B = (int)pow(2,L-1);	
		for(j=0;j<=B-1;j++)		
		//j = 0,1,2,...,2^(L-1) - 1
		{
			k=1;
			k = (int)pow(2,M-L);
			p=1;
			p=j*k;

			for(i=0; i<=k-1;i++) 
				{
					r=1;
					r=j+2*B*i;
					Tr=dataR[r+B]*cos(2.0*PI*p/N) - dataI[r+B]*sin(2.0*PI*p/N);
					Ti=dataI[r+B]*cos(2.0*PI*p/N) + dataR[r+B]*sin(2.0*PI*p/N);
					dataR[r+B]=dataR[r]-Tr;dataR[r+B]=dataR[r+B]/2; 
					dataI[r+B]=dataI[r]-Ti;dataI[r+B]=dataI[r+B]/2;
					dataR[r]=dataR[r]+Tr;dataR[r]=dataR[r]/2;
					dataI[r]=dataI[r]+Ti;dataI[r]=dataI[r]/2;
				}	
			}
	}
	for ( i=0;i<N;i++ )
	{ 		
	 	dataA[i]=sqrt(dataR[i]*dataR[i]+dataI[i]*dataI[i]);
	}
}

void IFFTR(float dataR[],float dataI[],float dataA[],int N,int M)
{
	int i,k;
	int j,r,p,B;
	float Tr,Ti;
	float yR[N/2],yI[N/2],yA[N/2]; 
	float x1R[N/2],x2R[N/2],x1I[N/2],x2I[N/2],xR[N],xI[N];
	for(i=0;i<N/2;i++)
	{
		yR[i]=dataR[2*i];//printf("%f\n",yR[i]);
		yI[i]=dataR[2*i+1];//printf("%f\n",yI[i]);
				
	}
	IFFT(yR,yI,yA,N/2,M-1);
	for(k=0;k<N/2;k++)
	{
		printf("%d,%f\n",N/2,yR[k]);
		//printf("%f\n",yI[N/2]);
	}
	for(k=0;k<N/2;k++)
	{
		if(k==0)
		{
			x1R[k]=yR[k];
			x1I[k]=yI[k];
			x2R[k]=yI[k];
			x2I[k]=-yR[k];	
		}
		else
		{
			x1R[k]=(yR[k]+yR[N/2-k])/2;
			x1I[k]=(yI[k]-yI[N/2-k])/2;
			x2R[k]=(yI[k]+yI[N/2-k])/2;
			x2I[k]=(yR[N/2-k]-yR[k])/2;
		}
	
	}
	
	B = 1;
	B = (int)pow(2,M-1);	
	for(j=0;j<=B-1;j++)		
	//j = 0,1,2,...,2^(L-1) - 1
	{		
		p=1;
		p=j;
		r=1;
		r=j;
		Tr=x2R[r]*cos(2.0*PI*p/N) - x2I[r]*sin(2.0*PI*p/N);
		Ti=x2I[r]*cos(2.0*PI*p/N) + x2R[r]*sin(2.0*PI*p/N);
		dataR[r]=x1R[r]+Tr;dataR[r]=dataR[r]/2;
		dataI[r]=x1I[r]+Ti;dataI[r]=dataI[r]/2;

		if(r==0)
		{
			dataR[N/2]=x1R[0]-x2R[0];dataR[N/2]=dataR[N/2]/2;
			dataI[N/2]=x1I[0]-x2I[0];dataI[N/2]=dataI[N/2]/2;	
		}
		else
		{
			dataR[N-r]=dataR[r];
			dataI[N-r]=-dataI[r];	
		}
	}
	

	for ( i=0;i<N;i++ )
	{ 		
		//dataA[i]=sqrt(xR[i]*xR[i]+xI[i]*xI[i]);
	 	dataA[i]=sqrt(dataR[i]*dataR[i]+dataI[i]*dataI[i]);
	}	
}  
